/******************************************************************************
 * Copyright (C) Ultraleap, Inc. 2011-2020.                                   *
 *                                                                            *
 * Use subject to the terms of the Apache License 2.0 available at            *
 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement           *
 * between Ultraleap and you, your company or other organization.             *
 ******************************************************************************/


using System;
using System.Collections.Generic;
using UnityEngine;

namespace Leap.Interaction.Internal.InteractionEngineUtility {
  public static class ColliderUtil {

    //*************
    //GENRAL HELPER

    /// <summary>
    /// Returns whether or not a given vector is within a given radius to a center point
    /// </summary>
    public static bool WithinDistance(Vector3 point, Vector3 center, float radius) {
      return (point - center).sqrMagnitude < radius * radius;
    }

    public static bool WithinDistance(Vector3 pointToOrigin, float radius) {
      return pointToOrigin.sqrMagnitude < radius * radius;
    }

    /// <summary>
    /// Given a point and a center, return a new point that is along the axis created by the 
    /// two vectors, and is a given distance from the center
    /// </summary>
    public static Vector3 GetPointAtDistance(Vector3 point, Vector3 center, float distance) {
      return (point - center).normalized * distance + center;
    }

    /// <summary>
    /// Given a collider and a position relative to the colliders transform, return whether or not the 
    /// position lies within the collider.  Can also optionally extrude the collider.
    /// 
    /// Warning: MeshColliders are not fully supported; this will only support testing against a MeshCollider's
    /// bounding box, with no extrusion.
    /// </summary>
    public static bool IsPointInside(this Collider collider, Vector3 localPosition, float extrude = 0.0f) {
      if (collider is SphereCollider) return (collider as SphereCollider).IsPointInside(localPosition, extrude);
      if (collider is BoxCollider) return (collider as BoxCollider).IsPointInside(localPosition, extrude);
      if (collider is CapsuleCollider) return (collider as CapsuleCollider).IsPointInside(localPosition, extrude);
      if (collider is MeshCollider) return collider.bounds.Contains(collider.transform.TransformPoint(localPosition));

      throw nullOrInvalidException(collider);
    }

    /// <summary>
    /// Given a collider and a position relative to the colliders transform, return the point on the surface 
    /// of the collider closest to the position.  Can also optionally extrude the collider.
    /// 
    /// Warning: If you are using a version of Unity pre-5.6, MeshColliders are not fully supported; this will
    /// only find the closest point on a MeshCollider's bounding box, with no extrusion.
    /// </summary>
    public static Vector3 ClosestPointOnSurface(this Collider collider, Vector3 localPosition, float extrude = 0.0f) {
      if (collider is SphereCollider) return (collider as SphereCollider).ClosestPointOnSurface(localPosition, extrude);
      if (collider is BoxCollider) return (collider as BoxCollider).ClosestPointOnSurface(localPosition, extrude);
      if (collider is CapsuleCollider) return (collider as CapsuleCollider).ClosestPointOnSurface(localPosition, extrude);
      if (collider is MeshCollider) return collider.transform.InverseTransformPoint(collider.ClosestPointOnBounds(collider.transform.TransformPoint(localPosition)));

      throw nullOrInvalidException(collider);
    }

    /// <summary>
    /// Given a list of colliders and a position in global space, return whether or not the position lies
    /// within any of the colliders.  Can also optionally extrude the collider.
    /// </summary>
    public static bool IsPointInside(List<Collider> colliders, Vector3 globalPosition, float extrude = 0.0f) {
      for (int i = 0; i < colliders.Count; i++) {
        Collider collider = colliders[i];
        Vector3 localPosition = collider.transform.InverseTransformPoint(globalPosition);
        if (collider.IsPointInside(localPosition, extrude)) {
          return true;
        }
      }
      return false;
    }

    /// <summary>
    /// Given a list of colliders and a position in global space, return the point on the surface of any
    /// of the colliders that is closest to the given position.  NOTE that this point might be inside of 
    /// a collider!  Can also optionally extrude the colliders.
    /// </summary>
    public static Vector3 ClosestPointOnSurfaces(List<Collider> colliders, Vector3 globalPosition, float extrude = 0.0f) {
      Transform chosenTransform = null;
      Vector3 closestPoint = Vector3.zero;
      float closestDistance = float.MaxValue;

      for (int i = 0; i < colliders.Count; i++) {
        Collider collider = colliders[i];
        Vector3 localPoint = collider.transform.InverseTransformPoint(globalPosition);
        Vector3 point = collider.ClosestPointOnSurface(localPoint, extrude);
        float distance = (globalPosition - point).sqrMagnitude;
        if (distance < closestDistance) {
          chosenTransform = collider.transform;
          closestDistance = distance;
          closestPoint = point;
        }
      }

      return chosenTransform.TransformPoint(closestPoint);
    }

    //***************
    //Raycasting

    public static bool SegmentCast(List<Collider> colliders, Vector3 worldStart, Vector3 worldEnd, out RaycastHit hitInfo) {
      Ray ray = new Ray(worldStart, worldEnd - worldStart);
      float dist = Vector3.Distance(worldStart, worldEnd);

      hitInfo = new RaycastHit();

      bool hitAny = false;
      RaycastHit tempHit;
      foreach (Collider collider in colliders) {
        if (collider.Raycast(ray, out tempHit, dist)) {
          if (!hitAny || tempHit.distance < hitInfo.distance) {
            hitInfo = tempHit;
            hitAny = true;
          }
        }
      }

      return hitAny;
    }

    //***************
    //SPHERE COLLIDER

    public static bool IsPointInside(this SphereCollider collider, Vector3 localPosition, float extrude = 0.0f) {
      localPosition -= collider.center;
      return WithinDistance(localPosition, collider.radius + extrude);
    }

    public static Vector3 ClosestPointOnSurface(this SphereCollider collider, Vector3 localPosition, float extrude = 0.0f) {
      return GetPointAtDistance(localPosition, collider.center, collider.radius + extrude);
    }

    //************
    //BOX COLLIDER

    public static bool IsPointInside(this BoxCollider collider, Vector3 localPosition, float extrude = 0.0f) {
      localPosition -= collider.center;
      if (Mathf.Abs(localPosition.x) > (collider.size.x / 2.0f) + (extrude / collider.transform.lossyScale.x)) {
        return false;
      }
      if (Mathf.Abs(localPosition.y) > (collider.size.y / 2.0f) + (extrude / collider.transform.lossyScale.y)) {
        return false;
      }
      if (Mathf.Abs(localPosition.z) > (collider.size.z / 2.0f) + (extrude / collider.transform.lossyScale.z)) {
        return false;
      }
      return true;
    }

    public static Vector3 ClosestPointOnSurface(this BoxCollider collider, Vector3 localPosition, float extrude = 0.0f) {
      localPosition -= collider.center;

      Vector3 radius = collider.size / 2.0f;
      radius.x += extrude;
      radius.y += extrude;
      radius.z += extrude;

      localPosition.x = Mathf.Clamp(localPosition.x, -radius.x, radius.x);
      localPosition.y = Mathf.Clamp(localPosition.y, -radius.y, radius.y);
      localPosition.z = Mathf.Clamp(localPosition.z, -radius.z, radius.z);

      //If an internal point
      if (Mathf.Abs(localPosition.x) < radius.x &&
          Mathf.Abs(localPosition.y) < radius.y &&
          Mathf.Abs(localPosition.z) < radius.z) {
        //Snap closest axis to the bounds
        //Farthest from the center
        if (Mathf.Abs(localPosition.x) > Mathf.Abs(localPosition.y)) {
          if (Mathf.Abs(localPosition.x) > Mathf.Abs(localPosition.z)) {
            //x is farthest
            localPosition.x = (localPosition.x > 0) ? radius.x : -radius.x;
          } else {
            //z is farthest
            localPosition.z = (localPosition.z > 0) ? radius.z : -radius.z;
          }
        } else {
          if (Mathf.Abs(localPosition.y) > Mathf.Abs(localPosition.z)) {
            //y is farthest
            localPosition.y = (localPosition.y > 0) ? radius.y : -radius.y;
          } else {
            //z is farthest
            localPosition.z = (localPosition.z > 0) ? radius.z : -radius.z;
          }
        }
      }

      localPosition += collider.center;
      return localPosition;
    }

    //****************
    //CAPSULE COLLIDER

    /// <summary>
    /// A capsule is defined as a line segment with a radius.  This method returns the two endpoints 
    /// of that segment, as well as it's length, in local space.
    /// </summary>
    public static void GetSegmentInfo(this CapsuleCollider collider, out Vector3 localV0, out Vector3 localV1, out float length) {
      Vector3 axis = Vector3.right;
      if (collider.direction == 1) {
        axis = Vector3.up;
      } else {
        axis = Vector3.forward;
      }

      length = Mathf.Max(0, collider.height - collider.radius * 2);
      localV0 = axis * length / 2.0f + collider.center;
      localV1 = -axis * length / 2.0f + collider.center;
    }

    public static bool IsPointInside(this CapsuleCollider collider, Vector3 localPosition, float tolerance = 0.0f) {
      Vector3 v0, v1;
      float length;
      collider.GetSegmentInfo(out v0, out v1, out length);

      if (length == 0.0f) {
        return WithinDistance(localPosition, collider.radius + tolerance);
      }

      float t = Vector3.Dot(localPosition - v0, v1 - v0) / (length * length);
      if (t <= 0.0f) {
        return WithinDistance(localPosition, v0, collider.radius + tolerance);
      }
      if (t >= 1.0f) {
        return WithinDistance(localPosition, v1, collider.radius + tolerance);
      }

      Vector3 projection = v0 + t * (v1 - v0);
      return WithinDistance(localPosition, projection, collider.radius + tolerance);
    }

    public static Vector3 ClosestPointOnSurface(this CapsuleCollider collider, Vector3 localPosition, float extrude = 0.0f) {
      Vector3 v0, v1;
      float length;
      collider.GetSegmentInfo(out v0, out v1, out length);

      if (length == 0.0f) {
        return GetPointAtDistance(localPosition, collider.center, collider.radius);
      }

      float t = Vector3.Dot(localPosition - v0, v1 - v0) / (length * length);
      if (t <= 0.0f) {
        return GetPointAtDistance(localPosition, v0, collider.radius + extrude);
      }
      if (t >= 1.0f) {
        return GetPointAtDistance(localPosition, v1, collider.radius + extrude);
      }

      Vector3 projection = v0 + t * (v1 - v0);
      return GetPointAtDistance(localPosition, projection, collider.radius + extrude);
    }

    public static float DistanceToSegment(Vector3 a, Vector3 b, Vector3 p) {
      float t = Vector3.Dot(p - a, b - a) / Vector3.Dot(a - b, a - b);
      if (t <= 0.0f) {
        return Vector3.Distance(p, a);
      }
      if (t >= 1.0f) {
        return Vector3.Distance(p, b);
      }
      Vector3 projection = a + t * (b - a);
      return Vector3.Distance(p, projection);
    }

    private static Exception nullOrInvalidException(Collider collider) {
      if (collider == null) {
        return new ArgumentNullException();
      } else {
        return new ArgumentException("Collider type of " + collider.GetType() + " is not supported.");
      }
    }
  }
}
